#include "Mavlinktest.h"
#include "QGCApplication.h"
#include "UAS.h"
#include "MAVLinkInspectorController.h"
#include "MultiVehicleManager.h"
#include <QtCharts/QLineSeries>
Mavlinktest::Mavlinktest()
    : QStringListModel(),
      _cursor_home_pos{-1},
      _cursor{0},
      _vehicle{nullptr}
{
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &Mavlinktest::_setActiveVehicle);
    _setActiveVehicle(manager->activeVehicle());
    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
    connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &Mavlinktest::_receiveMessage);
}

Mavlinktest::~Mavlinktest()
{
    if (_vehicle)
    {
        QByteArray msg;
        _sendSerialData(msg, true);
    }
}

void
Mavlinktest::sendCommand(QString command)
{
    _history.append(command);
    command.append("\n");
    _sendSerialData(qPrintable(command));
    _cursor_home_pos = -1;
    _cursor = rowCount();
}

QString
Mavlinktest::historyUp(const QString& current)
{
    return _history.up(current);
}

QString
Mavlinktest::historyDown(const QString& current)
{
    return _history.down(current);
}

void
Mavlinktest::_setActiveVehicle(Vehicle* vehicle)
{
    for (auto &con : _uas_connections)
    {
        disconnect(con);
    }
    _uas_connections.clear();

    _vehicle = vehicle;

    if (_vehicle)
    {
        _incoming_buffer.clear();
        // Reset the model
        setStringList(QStringList());
        _cursor = 0;
        _cursor_home_pos = -1;
        _uas_connections << connect(_vehicle, &Vehicle::mavlinkSerialControl, this, &Mavlinktest::_receiveData);
    }
}

void
Mavlinktest::_receiveData(uint8_t device, uint8_t, uint16_t, uint32_t, QByteArray data)
{
    if (device != SERIAL_CONTROL_DEV_SHELL)
    {
        return;
    }
   // auto idx = index(_cursor);
//setData(idx,  QString("%1 ttyS6 -> * [%2]").arg(QTime::currentTime().toString("HH:mm:ss.zzz")).arg(12));


    // Append incoming data and parse for ANSI codes
    _incoming_buffer.append(data);
    while(!_incoming_buffer.isEmpty())
    {
        bool newline = false;
        int idx = _incoming_buffer.indexOf('\n');
        if (idx == -1)
        {
            // Read the whole incoming buffer
            idx = _incoming_buffer.size();
        }
        else
        {
            newline = true;
        }

        QByteArray fragment = _incoming_buffer.mid(0, idx);
        if (_processANSItext(fragment))
        {
            writeLine(_cursor, fragment);
            if (newline)
            {
                _cursor++;
            }
            _incoming_buffer.remove(0, idx + (newline ? 1 : 0));
        }
        else
        {
            // ANSI processing failed, need more data
            return;
        }
    }
}

void
Mavlinktest::_receiveMessage(LinkInterface*, mavlink_message_t message)
{

    if(message.msgid==MAVLINK_MSG_ID_TEST_MAVLINK)
    {
        qDebug()<<"receive MAVLINK_MSG_ID_TEST_MAVLINK";
        mavlink_test_mavlink_t mavlink_test;

        mavlink_msg_test_mavlink_decode(&message, &mavlink_test);

//id=pwm.base_mode;
//_name=QString::number(heartbeat.base_mode);
        _test1=QString::number(mavlink_test.test1);
        _test2=QString::number(mavlink_test.test2);
        _test3=QString::number(mavlink_test.test3);
        emit valueChanged();
        qDebug()<<_test1<<_test2<<_test3;
//        qDebug()<<"从模式"<<heartbeat.custom_mode;
    }
}


void Mavlinktest::_sendcom(QString test1,QString test2,QString test3)
{

    if (!_vehicle)
    {
        qWarning() << "Internal error";
        return;
    }

    WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
    if (!weakLink.expired()) {
        SharedLinkInterfacePtr sharedLink = weakLink.lock();

        if (!sharedLink) {
            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
            return;
        }
//            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
            auto priority_link =sharedLink;

            uint8_t send_test1=test1.toUInt();
            int16_t send_test2=test2.toShort();
            float send_test3=test3.toFloat();
            mavlink_message_t msg;
            mavlink_msg_test_mavlink_pack_chan(_vehicle->id(),
                                               1,
                                               priority_link->mavlinkChannel(),
                                               &msg,
                                               send_test1,
                                               send_test2,
                                               send_test3);
           _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
            qDebug()<<"send"<<send_test1<<send_test2<<send_test3;
        }
}
void
Mavlinktest::_sendSerialData(QByteArray data, bool close)
{
    if (!_vehicle)
    {
        qWarning() << "Internal error";
        return;
    }

    WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
    if (!weakLink.expired()) {
        SharedLinkInterfacePtr sharedLink = weakLink.lock();

        if (!sharedLink) {
            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
            return;
        }


        // Send maximum sized chunks until the complete buffer is transmitted
//        while(data.size())
//        {
//            QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
//            uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE |  SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
//            if (close)
//            {
//                flags = 0;
//            }
//            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
//            auto priority_link =sharedLink;
//            mavlink_message_t msg;



//            mavlink_msg_serial_control_pack_chan(
//                protocol->getSystemId(),
//                protocol->getComponentId(),
//                priority_link->mavlinkChannel(),
//                &msg,
//                SERIAL_CONTROL_DEV_SHELL,
//                flags,
//                0,
//                0,
//                chunk.size(),
//                reinterpret_cast<uint8_t*>(chunk.data()));
//            _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
//            data.remove(0, chunk.size());
//        }
    }




}

bool
Mavlinktest::_processANSItext(QByteArray &line)
{
    // Iterate over the incoming buffer to parse off known ANSI control codes
    for (int i = 0; i < line.size(); i++)
    {
        if (line.at(i) == '\x1B')
        {
            // For ANSI codes we expect at least 3 incoming chars
            if (i < line.size() - 2 && line.at(i+1) == '[')
            {
                // Parse ANSI code
                switch(line.at(i+2))
                {
                    default:
                        continue;
                    case 'H':
                        if (_cursor_home_pos == -1)
                        {
                            // Assign new home position if home is unset
                            _cursor_home_pos = _cursor;
                        }
                        else
                        {
                            // Rewind write cursor position to home
                            _cursor = _cursor_home_pos;
                        }
                        break;
                    case 'K':
                        // Erase the current line to the end
                        if (_cursor < rowCount())
                        {
                            setData(index(_cursor), "");
                        }
                        break;
                    case '2':
                        // Check for sufficient buffer size
                        if ( i >= line.size() - 3)
                        {
                            return false;
                        }

                        if (line.at(i+3) == 'J' && _cursor_home_pos != -1)
                        {
                            // Erase everything and rewind to home
                            bool blocked = blockSignals(true);
                            for (int j = _cursor_home_pos; j < rowCount(); j++)
                            {
                                setData(index(j), "");
                            }
                            blockSignals(blocked);
                            QVector<int> roles;
                            roles.reserve(2);
                            roles.append(Qt::DisplayRole);
                            roles.append(Qt::EditRole);
                            emit dataChanged(index(_cursor), index(rowCount()), roles);
                        }
                        // Even if we didn't understand this ANSI code, remove the 4th char
                        line.remove(i+3,1);
                        break;
                }
                // Remove the parsed ANSI code and decrement the bufferpos
                line.remove(i, 3);
                i--;
            }
            else
            {
                // We can reasonably expect a control code was fragemented
                // Stop parsing here and wait for it to come in
                return false;
            }
        }
    }
    return true;
}

void
Mavlinktest::writeLine(int line, const QByteArray &text)
{
    auto rc = rowCount();
    if (line >= rc)
    {
        insertRows(rc, 1 + line - rc);
    }
    auto idx = index(line);
    setData(idx, data(idx, Qt::DisplayRole).toString() + text);
}

void Mavlinktest::CommandHistory::append(const QString& command)
{
    if (command.length() > 0)
    {

        // do not append duplicates
        if (_history.length() == 0 || _history.last() != command)
        {

            if (_history.length() >= maxHistoryLength)
            {
                _history.removeFirst();
            }
            _history.append(command);
        }
    }
    _index = _history.length();
}

QString Mavlinktest::CommandHistory::up(const QString& current)
{
    if (_index <= 0)
    {
        return current;
    }

    --_index;
    if (_index < _history.length())
    {
        return _history[_index];
    }
    return "";
}

QString Mavlinktest::CommandHistory::down(const QString& current)
{
    if (_index >= _history.length())
    {
        return current;
    }

    ++_index;
    if (_index < _history.length())
    {
        return _history[_index];
    }
    return "";
}






